/*
 * @Author: EltonLi lijiaming@uavi-tech.com
 * @Date: 2023-09-12 11:15:12
 * @LastEditors: LVGRAPE
 * @LastEditTime: 2025-06-04 10:25:59
 * @FilePath: \zino-fc-v4\applications\ZINO_FC_V4.0\statecalculate\statecal.c
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#include "statecal.h"
#include "fc_sensors.h"
#include "maths.h"
#include "zino.h"
#include "shanwai.h"
#include "pid.h"
#include "filter.h"
#include "fc_error.h"
#define DBG_TAG "statecal"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>
#include "usercmd.h"

#define GRAVITY_CMSS (980.f) // 单位：cm/s²
#define STATE_CAL_RATE 0.01
#define TOF_RANGE_MAX 5000
#define TOF_VARIANCE_SAMPLE 50
#define OPT_SACAL (0.01F)
#define VEL_NOSISE (0.01F)
extern float F1_wave[12];
// static float wBaro = 0.35f;
static float wBaroP = 0.021f;
static float wBaroV = 0.4f;
static float wOpflowV = 6.f;
static float wOpflowP = 0.5f;
variance_i16t tof_variance;
int16_t tof_var_buffer[TOF_VARIANCE_SAMPLE];
// static bool UseTOF = true;
// static bool UseTOF = false;
// static bool UseOpFlow = true;
static bool isRstHeight = false;
static bool isTofVanInit = false;
// static bool isRstAll = false;
int16_t tofChanges;
int16_t tof_old = 0;
int32_t tof_fix = 0;
float tof_height = 0;
float tof_height_lpf = 0;
estimator_t zroneEstimator;

int16_t optQuality_lpf = 0;
float fusedHeightLpf = 0.f;
float fusedHeightLpf_pre = 0.f;
float accVelZ = 0.f;
float accVelZ_lpf = 0.f;
float accVelZ_old = 0.f;
float weight = 0.1f;
float fusedHeight;
float fuseVelZ = 0.f;
float fuseVelZ_lpf = 0.f;
float optObserveHeight = 0.f;
float optObserveHeight_lpf = 0.f;
static float baroGroundAltittude = 0.f;
int16_t tofVariance = 0;
static bool accZ_overflow = false;
static uint8_t accZ_overflow_cnt = 0;
// static float startTofAsl = 0.f;
float zVel, acc_vel;
static uint32_t tof_stableT = 0;
static float vx_pre, vy_pre;
float accy_lpf, accx_lpf;
biquadFilter_t velZ_filter;
biquadFilter_t velBaro_filter;
biquadFilter_t velP_filter;
biquadFilter_t laser_filter;
biquadFilter_t vx_filter;
biquadFilter_t vy_filter;
biquadFilter_t ax_filter;
biquadFilter_t ay_filter;

float get_accy_lpf(void)
{
    return accy_lpf;
}
float get_accx_lpf(void)
{
    return accx_lpf;
}
// pt1Filter_t velZ_lpf_filter;
// static float baro_pos_old = 0.f;
// static float zVel_lpf = 0.f;
// static float acc_pos_old = 0.f;
float estimateVelZ = 0.f;
// 函数inavFilterPredict用于预测位置和速度
float get_tof_height_lfp(void)
{
    return tof_height_lpf;
}
void inavFilterPredict(int axis, float dt, float acc)
{
    // 根据加速度和当前速度，预测位置
    zroneEstimator.pos[axis] += (zroneEstimator.vel[axis] * dt + acc * dt * dt * 0.5f);
    // 根据加速度，预测速度
    zroneEstimator.vel[axis] += (acc * dt);
}
/*位置校正*/
void inavFilterCorrectPos(int axis, float dt, float e, float w)
{
    // float ewdt = e * w * dt;
    zroneEstimator.pos[axis] += e * w;
    // zroneEstimator.vel[axis] += w * ewdt;
}

/*速度校正*/
void inavFilterCorrectVel(int axis, float dt, float e, float w)
{
    zroneEstimator.vel[axis] += e * w * dt;
}

void positionEstimate(fc_sensor_data_t *sensor, m_imu_t *imu, zdrone_state_t *state, float dt)
{
    // float fusedHeightLpf = 0.f; /*融合高度，低通*/
    if (!isTofVanInit)
    {
        biquadFilterInitLPF(&velZ_filter, 10, 10000);
        biquadFilterInitLPF(&velP_filter, 10, 10000);
        biquadFilterInitLPF(&velBaro_filter, 100, 10000);
        biquadFilterInitLPF(&laser_filter, 10, 10000);
        biquadFilterInitLPF(&vx_filter, 10, 10000);
        biquadFilterInitLPF(&vy_filter, 10, 10000);
        biquadFilterInitLPF(&ax_filter, 10, 10000);
        biquadFilterInitLPF(&ay_filter, 5, 10000);
        variance_init_i16t(&tof_variance, tof_var_buffer, TOF_VARIANCE_SAMPLE);
        isTofVanInit = true;
    }
    if (FC_ERROR_CHECK(FC_IMU_CALIBRATING))
    {
        fusedHeight = 0;
        zroneEstimator.acc[Z] = 0;
        zroneEstimator.vel[Z] = 0;
        zroneEstimator.pos[Z] = 0;
        zroneEstimator.acc[X] = 0;
        zroneEstimator.vel[X] = 0;
        zroneEstimator.pos[X] = 0;
        zroneEstimator.acc[Y] = 0;
        zroneEstimator.vel[Y] = 0;
        zroneEstimator.pos[Y] = 0;

        sensor->optf_raw.fixPosX = 0;
        sensor->optf_raw.fixPosY = 0;
        state->pos.x = 0;
        state->pos.y = 0;

        isRstHeight = true;
        setPoint.Angle.yaw = 0;
        tof_height = sensor->tof.range;
        tof_height_lpf = sensor->tof.range;

        return;
    }

    // 获取地面高度
    if (isRstHeight && (millis() > 500)) // wait for baro until stable
    {
        isRstHeight = false;
        tof_fix = 0;
        baroGroundAltittude = sensor->baro.altitude;
    }

    float relateHight = sensor->baro.altitude - baroGroundAltittude;
    fusedHeight = relateHight;

    fusedHeightLpf += (fusedHeight - fusedHeightLpf) * 0.9f; /*融合高度 低通*/
    fuseVelZ = constrainf(((fusedHeight - fusedHeightLpf_pre) / dt), -100, 100);
    fusedHeightLpf_pre = fusedHeight;
    fuseVelZ_lpf += (fuseVelZ - fuseVelZ_lpf) * 0.9f;
    // fuseVelZ_lpf =biquadFilterApply(&velBaro_filter, fuseVelZ);
    // fuseVelZ_lpf = constrainf(fuseVelZ_lpf, -100, 100);

    tofChanges = (int16_t)sensor->tof.range - tof_old;
    tof_old = sensor->tof.range;
    tof_height_lpf = biquadFilterApply(&laser_filter, tof_height);
    if (sensor->tof.quality > 10 && sensor->tof.range < TOF_RANGE_MAX && sensor->tof.range > 1 && laser_online)
    {
        if (tof_stableT == 0)
        {
            tof_stableT = millis();
        }
        // NOTE laser is good

        if ((tof_stableT != 0) && (millis() > (tof_stableT + 3000)))
        {
            if (tofChanges < 50 && tofChanges > -50)
            {
                // tof_fix += (tofChanges * getCosTiltAngle());
                tof_fix += (tofChanges);
                tof_height = (tof_fix * 0.1f); // cm
            }

            // tof_height_lpf += (tof_height - tof_height_lpf) * 0.1f;

            float dtTofAndPosZ = tof_height_lpf - zroneEstimator.pos[Z];
            // if ((ABS(dtTofAndPosZ) > 5) && (millis() > 500))
            // {

            //     dtTofAndPosZ = 0;
            //     // dtTofAndPosZ = 0;
            // }

            inavFilterCorrectPos(Z, dt, dtTofAndPosZ, 0.8f);
            optObserveHeight = (sensor->tof.range * 0.1f); // cm
        }
        else
        {
            tof_fix = zroneEstimator.pos[Z] * 10.f;
            tof_height = zroneEstimator.pos[Z];
        }
        // tofVariance = variance_cal_i16t(&tof_variance, sensor->tof.range);
        // if (tofVariance < 10) // new stable platform2
        // {
        //     // float baro_tof_err = tof_height_lpf - fusedHeightLpf;
        //     // baroGroundAltittude -= baro_tof_err * 0.1f; // fixing baro's bias
        // }
    }
    else
    {

        tof_fix = zroneEstimator.pos[Z] * 10.f;
        tof_height = zroneEstimator.pos[Z];

        // NOTE laser is bad, laser's outdoor range only 20cm
        // NOTE USING BARO AS HEIGHT, opticalflow is only works below 5m
        optObserveHeight = constrainf(zdrone_state.pos.z - get_ground_height(), 10, 500);
        tof_stableT = 0;
    }

    //*cm = (float)(raw)*height_m / 11.914f * 2.54f; /* convert */
    // optObserveHeight_lpf = (optObserveHeight - optObserveHeight_lpf) * 0.5f;
    optObserveHeight_lpf += (optObserveHeight - optObserveHeight_lpf) * 0.1f;
    // float optVel_x = sensor->optf_raw.fixVelX * optObserveHeight_lpf * 0.01f * 7.754f;
    // float optVel_x = sensor->optf_raw.fixVelX * optObserveHeight_lpf * 0.01f * 7.754f;

    float optVel_x = sensor->optf_raw.fixVelX * optObserveHeight_lpf / 11.914f * 2.54f; /* convert */
    float optVel_y = sensor->optf_raw.fixVelY * optObserveHeight_lpf / 11.914f * 2.54f; /* convert */

    // if(optObserveHeight > 300)
    // {
    //     optVel_x = 0;
    //     optVel_y = 0;
    // }
    sensor->optf_raw.fixPosX += (optVel_x * dt);
    sensor->optf_raw.fixPosY += (optVel_y * dt);

    float optPos_x = sensor->optf_raw.fixPosX;
    float optPos_y = sensor->optf_raw.fixPosY;

    const float sensor_pos[3] = {optPos_x, optPos_y, fusedHeightLpf};
    const float sensor_vel[3] = {optVel_x, optVel_y, fuseVelZ_lpf};
    // const float sensor_vel[3] = {0, 0, fuseVelZ_lpf};
    float wbaroV_gain = (FC_ERROR_CHECK(FC_ARMED) ? 1.0f : 10.f);
    const float weight_pos[3] = {wOpflowP, wOpflowP, wBaroP};
    const float weight_vel[3] = {wOpflowV, wOpflowV, wBaroV * wbaroV_gain};
    /**
     * @brief
     * ^ X
     * |
     * |
     * |
     * --------->-Y
     */
    const uint16_t vel_limit[3] = {500, 500, 200};
    for (uint8_t i = 0; i < 3; i++)
    {
        if (fc_sensor_data.acc.sumCount.axis[i] == 0)
            fc_sensor_data.acc.sumCount.axis[i] = 1;

        zroneEstimator.acc[i] = (fc_sensor_data.acc.accSum.axis[i] / fc_sensor_data.acc.sumCount.axis[i]) * GRAVITY_CMSS;
        inavFilterPredict(i, fc_sensor_data.acc.sumTimer.axis[i], zroneEstimator.acc[i]);

        zroneEstimator.vel[i] = constrainf(zroneEstimator.vel[i], vel_limit[i] * -1, vel_limit[i]);

        inavFilterCorrectPos(i, fc_sensor_data.acc.sumTimer.axis[i], (sensor_pos[i] - zroneEstimator.pos[i]), weight_pos[i]);
        inavFilterCorrectVel(i, fc_sensor_data.acc.sumTimer.axis[i], (sensor_vel[i] - zroneEstimator.vel[i]), weight_vel[i]);

        fc_sensor_data.acc.accSum.axis[i] = 0;
        fc_sensor_data.acc.sumTimer.axis[i] = 0;
        fc_sensor_data.acc.sumCount.axis[i] = 0;
    }
    if (accZ_overflow)
    {
        fc_sensor_data.acc.accSum.axis[Z] = 0;
        fc_sensor_data.acc.sumCount.axis[Z] = 0;
        fc_sensor_data.acc.sumTimer.axis[Z] = 0;
        zroneEstimator.vel[Z] = 0;
    }
    if (opticalflow_online && sensor->optf_raw.isOpFlowOk)
    {
        optQuality_lpf += (sensor->optf_raw.optQual - optQuality_lpf) * 0.1f;
        // LOG_E("opticalflow_online and ok");
    }
    else
    {
        // optical flow don't work
        zroneEstimator.vel[X] = 0;
        zroneEstimator.vel[Y] = 0;
        // zroneEstimator.pos[X] = 0;
        // zroneEstimator.pos[Y] = 0;
        // LOG_E("opticalflow_online %d and not ok %d",opticalflow_online,sensor->optf_raw.isOpFlowOk);
    }

    static float posZ_pre = 0;
    static float velZ_lpf = 0;
    float velZ = (state->pos.z - posZ_pre) / dt;
    posZ_pre = state->pos.z;
    // velZ_lpf += (velZ - velZ_lpf) * 0.9f;
    velZ_lpf = biquadFilterApply(&velZ_filter, velZ);
    // velZ_lpf = pt1FilterApply2(&velZ_lpf_filter, velZ, 1, dt);

    F1_wave[0] = optVel_y * OPT_SACAL;
    F1_wave[1] = state->vel.y;
    F1_wave[2] = state->pos.y;
    F1_wave[3] = setPoint.Pos.y;
    F1_wave[4] = pidData[PID_POSITION_VY].outP;
    F1_wave[5] = pidData[PID_POSITION_VY].outI;
    F1_wave[6] = pidData[PID_POSITION_VY].outD;
    F1_wave[7] = pidData[PID_POSITION_VY].out;
    F1_wave[8] = pidData[PID_POSITION_Y].out;
    F1_wave[9] = optObserveHeight_lpf;
    F1_wave[10] = setPoint.Angle.roll;
    // F1_wave[11] = accy_lpf;
    // F1_wave[11] = tof_height_lpf;

    // F1_wave[0] = optVel_x;
    // F1_wave[1] = optVel_y;
    // F1_wave[2] = optPos_x;
    // F1_wave[3] = optPos_y;
    // F1_wave[4] = state->vel.x;
    // F1_wave[5] = state->vel.y;
    // // F1_wave[6] = optObserveHeight_lpf;
    // F1_wave[6] = pidData[PID_POSITION_VX].out;
    // F1_wave[7] = pidData[PID_POSITION_VY].out;
    // F1_wave[8] = setPoint.Pos.x;
    // F1_wave[9] = setPoint.Pos.y;
    // F1_wave[10] = state->pos.x;
    // F1_wave[11] = state->pos.y;

    // F1_wave[0] = zroneEstimator.acc[Z];
    // F1_wave[1] = zroneEstimator.vel[Z];
    // F1_wave[2] = fusedHeightLpf;
    // F1_wave[3] = fuseVelZ_lpf;
    // F1_wave[4] = tof_height_lpf;
    // F1_wave[5] = state->pos.z;
    // F1_wave[6] = velZ_lpf;
    // F1_wave[7] = tofVariance;
    // F1_wave[8] = pidData[PID_POSITION_VZ].outD;
    // F1_wave[9] = pidData[PID_POSITION_VZ].outI;
    // F1_wave[10] = pidData[PID_POSITION_VZ].out;

    // state->vel.x += constrainf((zroneEstimator.vel[X] * 0.01f - state->vel.x) * 0.9f, -5, 5);
    // state->vel.y += constrainf((zroneEstimator.vel[Y] * 0.01f - state->vel.y) * 0.9f, -5, 5);
    state->vel.x = constrainf(biquadFilterApply(&vx_filter, zroneEstimator.vel[X] * OPT_SACAL), -5.F, 5.F);
    state->vel.y = constrainf(biquadFilterApply(&vy_filter, zroneEstimator.vel[Y] * OPT_SACAL), -5.F, 5.F);
    state->vel.z = constrainf(velZ_lpf, -200, 200);

    state->vel.x = applyDeadbandFloat(state->vel.x, VEL_NOSISE);
    state->vel.y = applyDeadbandFloat(state->vel.y, VEL_NOSISE);

    state->pos.x = zroneEstimator.pos[X] * OPT_SACAL;
    state->pos.y = zroneEstimator.pos[Y] * OPT_SACAL;
    // state->pos.z = zroneEstimator.pos[Z];
    state->pos.z = biquadFilterApply(&velP_filter, zroneEstimator.pos[Z]);

    float acc_vy = (state->vel.y - vy_pre) / dt;
    vy_pre = state->vel.y;
    float acc_vx = (state->vel.x - vx_pre) / dt;
    vx_pre = state->vel.x;
    // accy_lpf = biquadFilterApply(&ay_filter, acc_vy);
    // accx_lpf = biquadFilterApply(&ax_filter, acc_vx);

    if (ABS(state->vel.z) > 95)
    {
        if (accZ_overflow_cnt < 100)
            accZ_overflow_cnt++;
        else
        {
            accZ_overflow = true;
        }
    }
    else
    {
        accZ_overflow_cnt = 0;
        accZ_overflow = false;
    }

    // state->pos.z = fc_sensor_data.tof.range / 10.0f;
}
// extern int baro_fusion(viod);
void statecal_task(void *p)
{
    while (1)
    {
        positionEstimate(&fc_sensor_data, &fc_ctrl_imu, &zdrone_state, STATE_CAL_RATE);
        // baro_fusion();
#if 0
        static uint8_t cnt = 0;
        if (cnt++ > 2)
        {
            cnt = 0;
            digitalOsc_Head();

            digitalOsc_printf("f1", (float)fc_sensor_data.optf_raw.optVelX);
            digitalOsc_printf("f1", (float)fc_sensor_data.optf_raw.optVelY);


            // digitalOsc_printf("f1", (float)fc_sensor_data.optf_raw.lpfX);
            // digitalOsc_printf("f1", (float)fc_sensor_data.optf_raw.lpfY);
            // digitalOsc_printf("f1", fc_sensor_data.optf.vel[X]);
            // digitalOsc_printf("f1", fc_sensor_data.optf.vel[Y]);
            // digitalOsc_printf("f1", zroneEstimator.acc[X]);
            // digitalOsc_printf("f1", zroneEstimator.acc[Y]);
            // digitalOsc_printf("f1", zroneEstimator.acc[Z]);
            // digitalOsc_printf("f1", zdrone_state.acc.z * GRAVITY_CMSS);

            digitalOsc_printf("f1", -zdrone_state.attitudeRate.pitch * 0.1);
            digitalOsc_printf("f1", -zdrone_state.attitudeRate.roll * 0.1);
            // digitalOsc_printf("f1", zroneEstimator.vel[Y]);
            digitalOsc_printf("f1", (float)fc_sensor_data.optf_raw.optVelX + zdrone_state.attitudeRate.pitch * 0.1);
            digitalOsc_printf("f1", (float)fc_sensor_data.optf_raw.optVelY + zdrone_state.attitudeRate.roll * 0.1);
            // digitalOsc_printf("f1", zroneEstimator.pos[X]);
            // digitalOsc_printf("f1", zroneEstimator.pos[Y]);
            // digitalOsc_printf("f1", fc_sensor_data.baro.altitude);
            // digitalOsc_printf("f1", baroGroundAltittude);
            // digitalOsc_printf("f1", fc_sensor_data.baro.altitude - baroGroundAltittude);
            // digitalOsc_printf("f1", fc_sensor_data.tof.range / 10.0f);
            // digitalOsc_printf("f1", fc_sensor_data.tof.quality / 100.0f);

            digitalOsc_End();
        }
#endif
        rt_thread_mdelay(10);
    }
}
void sensor_fusion(bool use_baro, bool use_tof, bool use_optflow)
{
    // static uint32_t last_time = 0;
    // float dt = (float)(rt_tick_get() - last_time)*0.001f;
    // static float baroAltitude_last, tofAltitude_last, optflowAltitude_last;
    // last_time = rt_tick_get();

    // if(use_baro && use_tof && use_optflow)
    // {

    // }
}
int statecal_task_entry(void)
{
    rt_thread_t state_update_thread = rt_thread_create("statecal", statecal_task, RT_NULL, 512, 10, 1);
    if (state_update_thread != RT_NULL)
    {
        rt_thread_startup(state_update_thread);
    }
    else
    {
        LOG_E("state_update_thread create failed.");
    }
    return RT_EOK;
}
// ZINO_APP_EXPORT(statecal_task_entry);

// MSH_CMD_EXPORT(statecal_task_entry, statecal_task);
